#include "common.h"

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <opencv2/opencv.hpp>

using namespace cv;
using namespace Eigen;

// 问题2：已知6个控制点在世界坐标系下的坐标和像素坐标，并且已知相机内参数矩阵K，求解旋转向量和偏移量R、T，m=K(R,t)X
// 旋转矩阵采用 rodrigues 公式计算三个参数
// Lambda Twist: An Accurate Fast Robust Perspective Three Point (P3P) Solver
// https://github.com/midjji/pnp

namespace pcv {

std::tuple<Vector3d, Vector3d> student_PnP(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d,
                                           const Matrix3d &K) {
    Matrix3d R = Matrix3d::Identity();
    Vector3d t = Vector3d::Zero();
    Vector3d r = rodrigues(R);

    // 1. 先采用 P3P 算法计算出四个可行解

    // 2. 利用任意第四个点，验证出唯一解

    // 3. 采用 ceres solver 进行最小二乘的迭代优化

    return std::make_tuple(r, t);
}

std::tuple<Vector3d, Vector3d> golden_PnP(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d,
                                          const Matrix3d &K) {
    Vector3d r(0.00495073771488337, 0.00495073771488337, 0.0495073771488337);
    Vector3d t(-99.8762512744035, 4.94744993043085, -0.507119865602733);

    return std::tuple<Vector3d, Vector3d>(r, t);
}

} // namespace pcv
